package Robot;

import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.Pose;
import Program.Listener;
import bluetooth.ProtocolPilot;

public class RobotListener extends Listener {

	private static final long STANDARD_REFRESH_RATE = 200;
	private long refreshRate;
	
	private RobotControl control;
	private ProtocolPilot pilot;
	
	private RobotDirection direction;
	private double sensorAngle;
	private boolean leftIsPressed;
	private boolean rightIsPressed;
	private int lightValue;
	private int sonarDistance;
	
	private long lastUpdate = 0;
	
	public RobotListener(ProtocolPilot pilot, RobotControl control) {
		this.control = control;
		this.pilot = pilot;
		resetWaitTime();
	}
	
	@Override
	public void setWaitTime(long rate) {
		refreshRate = rate;
	}
	
	@Override
	public void resetWaitTime() {
		refreshRate = STANDARD_REFRESH_RATE;
	}
	
	@Override
	public RobotDirection getDirection() {
		return direction;
	}
	
	@Override
	public double getSensorAngle() {
		return sensorAngle;
	}
	
	/**
	 * @return the leftIsPressed
	 */
	public boolean isLeftIsPressed() {
		return leftIsPressed;
	}

	/**
	 * @return the rightIsPressed
	 */
	public boolean isRightIsPressed() {
		return rightIsPressed;
	}

	/**
	 * @return the lightValue
	 */
	@Override
	public int getLightValue() {
		return lightValue;
	}
	
	/**
	 * @return the sonarDistance
	 */
	@Override
	public double getSonarDistance() {
		return sonarDistance;
	}
	
	@Override
	public int[] sonarPing() {
		/*
		sonar.ping();
		int[] result = new int[8];
		
		sonar.getDistances(result);
		control.setPingValues(result);
		*/
		int[] result = new int[8];
		return result;
	}

	private void updatePosition() {
		Pose pose = pilot.getPose();
		control.setRobotPose(pose);		
	}
	
	private void updateValues()
	{
		Move move = pilot.getMovement();
		if (move.getDistanceTraveled() > 0) 
			direction = RobotDirection.FORWARD;
		else if (move.getDistanceTraveled() < 0) 
			direction = RobotDirection.BACKWARD; 
		else if(move.getAngleTurned() > 0)
			direction = RobotDirection.RIGHT;
		else if(move.getAngleTurned() < 0)
			direction = RobotDirection.LEFT;
		else 
			direction = RobotDirection.NONE;
		sensorAngle = pilot.getSonarTachoCount();
//		//set the current status of the touch sensors
		leftIsPressed = pilot.isTouchLeftPressed();
		rightIsPressed = pilot.isTouchRightPressed();
		//set the current light status
		//lightValue = light.getLightValue();
		lightValue = pilot.getLightValue();
		// set the current sonar distance
		sonarDistance = (int) pilot.getSonarDistance();
		//update the RobotControl
		control.setCurrentDirection(direction);
		control.setSensorAngle(sensorAngle);
		control.setLightValue(lightValue);
		control.setSonarDistance(sonarDistance);
		control.setTouchLeft(leftIsPressed);
		control.setTouchRight(rightIsPressed);
	}
	
	public void waitForRefresh()
	{
		try {
			Thread.sleep(1000);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		/*
		long previousUpdate = lastUpdate;
		while(previousUpdate == lastUpdate)
			Thread.yield();
		*/
	}
	
	@Override
	public void run() {
		while (true) { // TODO aanpassen als we ook me arc werken
			// set the current direction of the robot
			/*
			
			*/
			// set the current angle of the sensor
			updateValues();
			updatePosition();
			lastUpdate = System.currentTimeMillis();
			try {
				Thread.sleep(refreshRate);
			} catch (InterruptedException e) {
				
			}
		}
	}
	
}
